/*
 * obstacleAvoidance.cpp
 *
 *  Created on: Jan 13, 2009
 */

/*
 * ScanMatchMain.cpp
 *
 *  Created on: Jul 6, 2008
 */

#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include <signal.h>

#include <lcm/lcm.h>
#include <bot_core/bot_core.h>
#include <bot_lcmgl_client/lcmgl.h>
#include <bot_param/param_client.h>

#include <lcmtypes/quad_lcmtypes.h>
#include <quad_controller/controller_interface.h>
#include <bot_frames/bot_frames.h>
#include <laser_utils/laser_util.h>

#include <getopt.h>
#include <GL/gl.h>

using namespace std;

////////////////////////////////////////////////////////
/////////////////////GLOBALS////////////////////////////
////////////////////////////////////////////////////////
static lcm_t * lcm;
static BotParam * bot_param;
static BotFrames *bot_frames;
static Laser_projector * laser_proj;
static bot_lcmgl_t * lcmgl;

static int scan_skip = 0;
static double laser_period = .025;  //40Hz default... updated with EMA
static double laser_period_ema_alpha = .001;
static double distToWallAvoidThresh;
static double distToWallStopThresh;
static double default_xy_vel;
static double max_avoid_vel;
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////

static void usage(char * funcName) {
  printf(
      "Usage: %s [options]\n"
      "options are:\n"
      "--help,          -h      display this message \n"
      "--scan_skip,    -s  skip this number of scans in between every processed scan defaults to 0\n",
      funcName);
  exit(1);

}

void read_parameters(int argc, char **argv) {

  const char *optstring = "hs:";
  struct option long_opts[] = { { "help", no_argument, 0, 'h' }, { "scan_skip",
      required_argument, 0, 's' }, { 0, 0, 0, 0 } };

  int c;
  while ((c = getopt_long(argc, argv, optstring, long_opts, 0)) >= 0) {
    switch (c) {
      case 'h':
        usage(argv[0]);
        break;
      case 's':
        scan_skip = atoi(optarg);
        if (scan_skip < 0)
          usage(argv[0]);
        laser_period /= (scan_skip + 1);
        break;
      default:
        usage(argv[0]);
        break;
    }
  }

  //get obstacle avoidance params
  distToWallAvoidThresh = bot_param_get_double_or_fail(
      bot_param, "obstacle_avoidance.distToWallAvoidThresh");
  distToWallStopThresh = bot_param_get_double_or_fail(
      bot_param, "obstacle_avoidance.distToWallStopThresh");
  default_xy_vel = bot_param_get_double_or_fail(bot_param,
                                                "controller.default_xy_vel");
  max_avoid_vel = default_xy_vel;
  fprintf(stderr, "Running with params:\n");
  fprintf(stderr, "distToWallAvoidThresh=%f\n", distToWallAvoidThresh);
  fprintf(stderr, "distToWallStopThresh=%f\n", distToWallStopThresh);
  fprintf(stderr, "default_xy_vel=%f\n", default_xy_vel);
  fprintf(stderr, "max_avoid_vel=%f\n", max_avoid_vel);

}

void lcmgl_draw_avoid_vec(point2d_t * sm_avoidVec) {

  double avoidVec_body[3] = { sm_avoidVec->x, sm_avoidVec->y, 0 };
  bot_vector_scale_3d(avoidVec_body, 500);
  double avoidVec_global[3];
  BotTrans body_to_local;
  bot_frames_get_trans(bot_frames, "body", bot_frames_get_root_name(bot_frames),
                       &body_to_local);
  bot_frames_transform_vec(bot_frames, "body",
                           bot_frames_get_root_name(bot_frames), avoidVec_body,
                           avoidVec_global);

  bot_lcmgl_color3f(lcmgl, 1, 0, 0);
  bot_lcmgl_point_size(lcmgl, 12);
  bot_lcmgl_line_width(lcmgl, 12);
  bot_lcmgl_begin(lcmgl, GL_LINES);
  bot_lcmgl_vertex3f(lcmgl, body_to_local.trans_vec[0],
                     body_to_local.trans_vec[1], body_to_local.trans_vec[2]);
  bot_lcmgl_vertex3f(lcmgl, avoidVec_global[0], avoidVec_global[1],
                     avoidVec_global[2]);
  bot_lcmgl_end(lcmgl);
  bot_lcmgl_switch_buffer(lcmgl);

}

void lcmgl_clear() {

  bot_lcmgl_begin(lcmgl, GL_LINES);
  bot_lcmgl_end(lcmgl);
  bot_lcmgl_switch_buffer(lcmgl);
}

void shutdown_handler(int unused __attribute__((unused))) {
  fprintf(stderr, "Shutting Down \n");
  lcmgl_clear();
  bot_tictoc_print_stats(BOT_TICTOC_AVG);
  exit(1);
}

static void waypoint_handler(const lcm_recv_buf_t *rbuf __attribute__((unused)),
                             const char * channel __attribute__((unused)),
                             const quad_waypoint_t * msg,
                             void * user __attribute__((unused))) {
  double cv = bot_vector_magnitude_2d(msg->xyzt_dot);
  max_avoid_vel = fmax(default_xy_vel, cv);
}

void laser_handler(const lcm_recv_buf_t *rbuf __attribute__((unused)),
                   const char * channel __attribute__((unused)),
                   const bot_core_planar_lidar_t * msg,
                   void * user __attribute__((unused))) {
  static int counter = 0;
  if (counter++ % (scan_skip + 1) != 0)
    return;

  bot_tictoc("per");
  bot_tictoc("handler");

  static int64_t lastHBPrintTime = -1;
  if (bot_timestamp_now() - lastHBPrintTime > 1e6) {
    lastHBPrintTime = bot_timestamp_now();
    fprintf(stderr, ".");
  }
  double maxRange = fmin(distToWallAvoidThresh * 3, laser_proj->max_range);
  int64_t handlerStartTime = bot_timestamp_now();

  //estimate the laser period
  int64_t utime = msg->utime;
  static int64_t utime_prev = utime - laser_period * 1e6;
  double dt = (utime - utime_prev) * 1e-6;
  laser_period = laser_period * (1 - laser_period_ema_alpha)
      + laser_period_ema_alpha * dt;
  utime_prev = utime;  //save for next time

  bot_tictoc("project");
  //project the scans
  BotTrans body_to_local;
  laser_projected_scan *lscan = laser_create_projected_scan_from_planar_lidar(
      laser_proj, msg, "body");
  bot_frames_get_trans_with_utime(bot_frames, "body",
                                  bot_frames_get_root_name(bot_frames),
                                  msg->utime, &body_to_local);
  //take out the yaw
  double rpy[3];
  bot_quat_to_roll_pitch_yaw(body_to_local.rot_quat, rpy);
  rpy[2] = 0;
  bot_roll_pitch_yaw_to_quat(rpy, body_to_local.rot_quat);

  int numValidPoints = 0;
  static point2d_t * points = (point2d_t *) calloc(lscan->npoints,
                                                   sizeof(point2d_t));
  double rotated_point[3];
  int lastAdd = -1000;
  double last_added_point[3];
  for (int i = 0; i < lscan->npoints; i++) {
    if (lscan->point_status[i] != laser_surround)
      continue;
    bot_trans_rotate_vec(&body_to_local, point3d_as_array(&lscan->points[i]),
                         rotated_point);
    memcpy(last_added_point, rotated_point, 3 * sizeof(double));
    points[numValidPoints].x = rotated_point[0];
    points[numValidPoints].y = rotated_point[1];
    numValidPoints++;
  }
  laser_destroy_projected_scan(lscan);
  bot_tictoc("project");

  //scan is projected with laser at (0,0) 0
  double minDist = 1000000;
  bot_tictoc("compute_avoidvec");
  point2d_t avoidVec = { 0, 0 };
  point2d_t origin = { 0, 0 };
  //TODO: is all this projection stuff worth it?
  for (int p = 0; p < numValidPoints; p++) {
    double dist = bot_vector_dist_2d(point2d_as_array(&origin),
                                     point2d_as_array(&points[p]) );
    if (dist < minDist)
      minDist = dist;
    if (dist < distToWallAvoidThresh) {
      point2d_t av = points[p];
      bot_vector_normalize_2d(point2d_as_array(&av) );
      //vector away from segment
      avoidVec.x += -av.x * (distToWallAvoidThresh - dist);
      avoidVec.y += -av.y * (distToWallAvoidThresh - dist);
    }

  }

  bot_tictoc("compute_avoidvec");

  bot_tictoc("normalize and send");
  double rateLimitFactor = 1;
  static int prev_too_close = 0;
  //create an apropriate relative waypoint if we're too close
  if (minDist < distToWallAvoidThresh) {

    //normalize the avoidance vector, and give it magnitude proportional to the minDist scaled by a rate limiting factor
    double rateLimitFactor = (distToWallAvoidThresh - minDist)
        / (distToWallAvoidThresh - distToWallStopThresh);
    double fullRate = laser_period * max_avoid_vel;
    double magnitude = rateLimitFactor * fullRate;

    bot_vector_normalize_2d(point2d_as_array(&avoidVec) );
    avoidVec.x *= magnitude;
    avoidVec.y *= magnitude;

    static int64_t lastPrintTime = -1;
    if (bot_timestamp_now() - lastPrintTime > 5e5) {
      lastPrintTime = bot_timestamp_now();
      fprintf(stderr,
              "We're too close to a wall... sending body_Relative Waypt!\n");
      fprintf(
          stderr,
          "minDist = %.2f avoidVec.x= %.2f, avoidVec.x=%.2f, laserPeriod=%.3f, max_avoid_vel=%.3f, rateLimitFactor=%.4f, magnitude=%.3f, fullRate=%.3f\n",
          minDist, avoidVec.x, avoidVec.y, laser_period, max_avoid_vel,
          rateLimitFactor, magnitude, fullRate);
    }
    carmen3d_controller_publish_body_relative_waypoint(lcm, avoidVec.x,
                                                       avoidVec.y, utime);
    lcmgl_draw_avoid_vec(&avoidVec);
    prev_too_close = 1;
  } else if (prev_too_close) {
    prev_too_close = 0;
    printf("clearning lcmgl\n");
    lcmgl_clear();
  }
  bot_tictoc("normalize and send");
  bot_tictoc("handler");
}
using namespace std;

int main(int argc __attribute__((unused)),
         char *argv[] __attribute__((unused))) {
  fprintf(stderr, "starting obstacle avoidance!\n");

  lcm = bot_lcm_get_global(NULL);
  bot_param = bot_param_new_from_server(lcm, 0);
  read_parameters(argc, argv);

  bot_frames = bot_frames_get_global(lcm, bot_param);
  laser_proj = laser_projector_new(bot_param, bot_frames, "laser", 0);  //TODO:don't hardcode

  lcmgl = bot_lcmgl_init(lcm, "ObstacleAvoidance");

  bot_core_planar_lidar_t_subscribe(lcm, LASER_CHANNEL, &laser_handler, NULL);
  quad_waypoint_t_subscribe(lcm, WAYPOINT_CURR_CHANNEL, waypoint_handler, NULL);

  signal(SIGINT, shutdown_handler);
  while (1)
    lcm_handle(lcm);
  return 0;
}

